
// F. Chaxel 2010

#include "Motors.h"

#include <iostream>
using namespace std;

Motors* Motors::_instance = NULL;

// I2C adress 
#define MotorControlMD23 0xB0

//////////////////////////////////////////////////////////////////////
// Construction/Destruction
//////////////////////////////////////////////////////////////////////

Motors::Motors()
{
	if (_instance!=NULL)	// I am a Singleton
		throw ;
	
	_instance=this;

	i2c=new I2C(USER_MODE_DRIVER);

	Stop();
	InitPosition();
	i2c->WriteByte (MotorControlMD23,16,49);	// automatic speed regulation
	i2c->WriteByte (MotorControlMD23,15,0);	// Mode 0 Speed Right and Left Command
}

Motors::~Motors()
{
	Stop();
	_instance=NULL;
}

//////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////
int Motors::GetBattery()
{
	return i2c->ReadByte(MotorControlMD23, 10);
}

void Motors::GetMotorsCurrent(unsigned char *Left, unsigned char *Right)
{
	*Left=i2c->ReadByte(MotorControlMD23, 11);
	*Right=i2c->ReadByte(MotorControlMD23, 12);

}
//////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////
void Motors::InitPosition()
{
	i2c->WriteByte (MotorControlMD23,16,32);	// reset encoder to 0

}
//////////////////////////////////////////////////////////////////////
void Motors::Stop()
{
	SetSpeed (128,128);
	cout << "stopped" << endl;
}
//////////////////////////////////////////////////////////////////////
void Motors::SetSpeed (unsigned int Left, unsigned int Right)
{
	short Val= ((Right<<8)&0xFF00)|(Left&0xFF);
	
	i2c->WriteWord (MotorControlMD23,0,Val);

}
/////////////////////////////////////////////////////////////////////
void Motors::GetPos(long *Left, long *Right)
{

	i2c->Read_two_Long(MotorControlMD23, 2, Left, Right);

}
